#include <Eigen/Core>
#include <filesystem>
#include <iostream>
#include <memory>
#include <vector>

#include "boost/algorithm/string.hpp"
#include "boost/json.hpp"
#include "common/map_utility.h"
#include "common/utility.h"
#include "filter/map_filter.h"
// #include "fit/dbscan.h"
// #include "fit/rectangle_fitting.h"
#include "perception_visualizer.h"
#include "perception_visualizer_debug.h"
// #include "track/tracker.h"
// #include "vis/paint.h"

int main(int argc, char const* argv[]) {
  try {
    // get conf file
    std::string conf_json = "../conf/test_conf.json";
    if (!std::filesystem::exists(conf_json)) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config file is not exists !" << std::endl;
      return (EXIT_FAILURE);
    }

    std::fstream conf_file;
    conf_file.open(conf_json, std::ios_base::in);
    if (!conf_file.is_open()) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Failed to open the config file !" << std::endl;
      return (EXIT_FAILURE);
    }

    // read conf json
    std::string conf_json_str;
    if (conf_file) {
      conf_file.seekg(0, std::ios::end);
      conf_json_str.resize(conf_file.tellg());
      conf_file.seekg(0, std::ios::beg);
      conf_file.read(&conf_json_str[0], conf_json_str.size());
      conf_file.close();
    } else {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Failed to read the config file !" << std::endl;
      return (EXIT_FAILURE);
    }

    boost::json::value conf_json_val = boost::json::parse(conf_json_str);
    boost::json::object conf_json_obj_root = conf_json_val.as_object();

    // get viewer style
    if (!conf_json_obj_root.contains("Viewer_Style")) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config json content does not contain the Viewer_Style !"
                << std::endl;
      return (EXIT_FAILURE);
    }
    boost::json::object viewer_style_obj =
        conf_json_obj_root["Viewer_Style"].as_object();

    // get map json folder path
    if (!conf_json_obj_root.contains("decode_data_root")) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config json content does not contain the decode_data_root !"
                << std::endl;
      return (EXIT_FAILURE);
    }
    std::string decodeDataRoot =
        conf_json_obj_root["decode_data_root"].as_string().c_str();

    // get ground truth map json folder path
    if (!conf_json_obj_root.contains("GroundTruth_Json_Dir_Path")) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config json content does not contain the "
                   "GroundTruth_Json_Dir_Path !"
                << std::endl;
      return (EXIT_FAILURE);
    }
    std::string ground_truth_dir_path =
        conf_json_obj_root["GroundTruth_Json_Dir_Path"].as_string().c_str();

    // get map json file list
    std::vector<std::string> json_file_list;
    auto const& mapDir{std::filesystem::path(decodeDataRoot) /
                       std::filesystem::path("map")};
    if (!std::filesystem::exists(mapDir)) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "map data path: " << mapDir << " does not exists !"
                << std::endl;
      return (EXIT_FAILURE);
    }
    for (const auto& entry : std::filesystem::directory_iterator(mapDir)) {
      if (std::filesystem::is_regular_file(entry.status())) {
        std::string file_path = entry.path().string();
        std::string file_type = file_path.substr(file_path.rfind('.'));
        if (boost::iequals(file_type, ".json")) {
          json_file_list.push_back(file_path);
        }
      }
    }
    std::sort(json_file_list.begin(), json_file_list.end());

    auto& perceptionVisualizer{
        autodrive::perception::PerceptionVisualizer::GetInstance()};
    if (!perceptionVisualizer.Init(viewer_style_obj)) {
      std::cerr << autodrive::perception::to_red("[Error] ")
                << "Config json exception, perceptionVisualizer init failed!"
                << std::endl;
      return (EXIT_FAILURE);
    }

    // auto& perceptionVisualizerDebug{
    //     autodrive::perception::PerceptionVisualizerDebug::GetInstance()};
    // perceptionVisualizerDebug.Init();

    // do work
    std::shared_ptr<autodrive::perception::Map> map_src_ptr =
        std::make_shared<autodrive::perception::Map>();
    std::shared_ptr<autodrive::perception::Map> map_filter_ptr =
        std::make_shared<autodrive::perception::Map>();
    std::shared_ptr<autodrive::perception::Map> map_truth_ptr =
        std::make_shared<autodrive::perception::Map>();
    // autodrive::perception::MapJsonLoader map_loader;

    autodrive::perception::MapFilter::MapFilterParameter mapFilterPara{
        .roi{.top_left{.x = -51.2, .y = 25.6},
             .bottom_right{.x = 153.6, .y = -25.6}},
        .match_paramemter{.roi{.top_left{.x = -51.2, .y = 25.6},
                               .bottom_right{.x = 153.6, .y = -25.6}},
                          .chamfer_distance_threshold = 2,
                          .min_distance_threshold = 0.8,
                          .max_distance_threshold = 3},
        .fusion_parameter{
            .x_low_limit = -51.2, .threshold = 100, .history_weight = 0.9},
        .lifetime_threshold = 5,
        .lifetime_bonus_threshold = 50};

    autodrive::perception::MapFilter map_filter;
    auto const& poseDir{std::filesystem::path(decodeDataRoot) /
                        std::filesystem::path("pose")};

    while (true) {
      for (auto& json_file : json_file_list) {
        // json_file = "../data/decodeResult/map/1695797056567377.json";
        std::cout << "file : " << json_file << std::endl;
        std::filesystem::path const& mapFile{json_file};
        if (!std::filesystem::exists(mapFile)) {
          std::cout << "[" << autodrive::perception::to_red("Error") << "] "
                    << "map file " << json_file << " does not exist!!!"
                    << std::endl;
          continue;
        }

        auto const& timeStampData{
            std::strtoull(mapFile.filename().stem().c_str(), 0, 0)};

        std::filesystem::path const& poseFile{
            poseDir /
            std::filesystem::path(std::to_string(timeStampData) + ".txt")};
        if (!std::filesystem::exists(poseFile)) {
          std::cout << "[" << autodrive::perception::to_red("Error") << "] "
                    << "pose file " << poseFile << " does not exist!!!"
                    << std::endl;
          continue;
        }

        std::int64_t const& timestamp{
            autodrive::perception::clip<std::int64_t>(timeStampData)};

        autodrive::perception::read_map_json(json_file, *map_src_ptr);
        autodrive::perception::read_map_json(json_file, *map_filter_ptr);

        std::vector<double> poseData;
        autodrive::perception::read_txt_data(poseFile, poseData);

        Eigen::Affine3d pose;
        pose.matrix() =
            Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>>(
                poseData.data());

        map_src_ptr->timestamp = timestamp;
        map_filter_ptr->timestamp = timestamp;

#if 1
        std::string json_file_name = json_file.substr(json_file.rfind('/') + 1);
        std::string ground_truth_json_path =
            ground_truth_dir_path + "/" + json_file_name;
        if (std::filesystem::exists(ground_truth_json_path)) {
          // map_loader.Load(ground_truth_json_path, *map_truth_ptr);
          autodrive::perception::read_map_json(ground_truth_json_path,
                                               *map_truth_ptr);
          map_truth_ptr->timestamp = timestamp;
        }
        autodrive::perception::Timer tRun;
        /****  do map filter  ****/

        map_filter.Init(mapFilterPara);
        map_filter(*map_filter_ptr, pose);

        // auto const& mapResultDir{"map"};
        // if (!std::filesystem::exists(mapResultDir)) {
        //   std::filesystem::create_directories(mapResultDir);
        // }
        // auto const& fileName{
        //     std::filesystem::path(mapResultDir) /
        //     std::filesystem::path(std::to_string(timestamp) + ".json")};
        // autodrive::perception::write_map_json(*map_filter_ptr, fileName);

        std::cout << "[Info] map filter Run time: "
                  << autodrive::perception::to_green(
                         std::to_string(tRun.elapsed() / (float)1000))
                  << " ms, " << std::endl
                  << std::endl;

        perceptionVisualizer.Enqueue(std::to_string(timestamp),
                                     map_src_ptr.get(), map_filter_ptr.get(),
                                     map_truth_ptr.get());
        // if (perceptionVisualizerDebug.IsDispose()) {
        //   break;
        // }
        std::chrono::milliseconds dura(100);
        std::this_thread::sleep_for(dura);
#else
        // visulization
        Paint p;
        p.Show();
        std::unique_ptr<autodrive::perception::Tracker> tracker =
            std::make_unique<autodrive::perception::Tracker>();
        tracker->Init("../conf");
        // std::cout << "json file = " << json_file << std::endl;
        // float turn_score_threshold = 0.51;   // score的阈值
        // float cross_score_threshold = 0.4;   // score的阈值
        // float nostop_score_threshold = 0.4;  // score的阈值
        // std::vector<Eigen::Vector2f> turn_points;
        // std::vector<Eigen::Vector2f> crosswalk_points;
        // std::vector<Eigen::Vector2f> nostop_points;

        // autodrive::perception::extract_map_roadmarking_data(
        //     *map_src_ptr, turn_points, crosswalk_points, nostop_points,
        //     turn_score_threshold, cross_score_threshold,
        //     nostop_score_threshold);

        // // DBSCAN
        // DBScan<Eigen::Vector2f> dbscan(1.0, 4);
        // // res[0]是噪点   其实是正常点
        // vector<vector<Eigen::Vector2f>> res =
        // dbscan.GetClusters(turn_points); std::cout << "DBSCAN count = " <<
        // res.size() << std::endl; RectangleFitting Fit; vector<cv::Mat> mats;
        // mats.resize(res.size() - 1);
        // for (int i = 1; i < res.size(); ++i) {
        //   const vector<Eigen::Vector2f>& points = res.at(i);
        //   RotatedRect rec;
        //   Fit.Fit(points, rec, mats[i - 1]);
        //   // std::cout << "mat = " << mats[i - 1] << std::endl;
        // }

        // p.Draw(res, mats);
        // // p.Draw(turn_points, res, mats);

        // tracker->Run(*map_src_ptr, pose);
        // p.Draw(*map_src_ptr);

#endif
      }
      // if (perceptionVisualizerDebug.IsDispose()) {
      //   break;
      // }
      // std::chrono::milliseconds dura(1000);
      // std::this_thread::sleep_for(dura);
    }
  } catch (const std::exception& e) {
    std::cerr << "run test error, " << e.what() << '\n';
  } catch (...) {
    std::cerr << "unexpected error!!!" << '\n';
  }
  exit(EXIT_SUCCESS);
}
